#include "IMU.hpp"
#include "REG.h"
#include "wit_c_sdk.h"
#include <cstddef>

#define ACC_UPDATE 0x01
#define GYRO_UPDATE 0x02
#define ANGLE_UPDATE 0x04
#define MAG_UPDATE 0x08
#define READ_UPDATE 0x80

static void StaticSensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum) {}

bool IMU::InitDevice(std::string dev, int baudrate)
{
    if (!serial.Open(dev, baudrate))
        return false;
    WitInit(WIT_PROTOCOL_NORMAL, 0x50);
    WitRegisterCallBack(StaticSensorDataUpdata);
    return true;
}

IMUState IMU::GetState()
{
    std::vector<char> mes = serial.Read();
    for (size_t i = 0; i < mes.size(); i++)
    {
        WitSerialDataIn(mes[i]);
    }
    IMUState state;
    for (size_t i = 0; i < 3; i++)
    {
        state.acc[i] = sReg[AX + i] / 32768.0f * 16.0f;
        state.gyro[i] = sReg[GX + i] / 32768.0f * 2000.0f;
        state.angle[i] = sReg[Roll + i] / 32768.0f * 180.0f;
    }
    return state;
}

void IMU::SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum)
{
    int i;
    for (i = 0; i < uiRegNum; i++)
    {
        switch (uiReg)
        {
            // case AX:
            // case AY:
        case AZ:
            dataUpdateFlag |= ACC_UPDATE;
            break;
            // case GX:
            // case GY:
        case GZ:
            dataUpdateFlag |= GYRO_UPDATE;
            break;
            // case HX:
            // case HY:
        case HZ:
            dataUpdateFlag |= MAG_UPDATE;
            break;
            // case Roll:
            // case Pitch:
        case Yaw:
            dataUpdateFlag |= ANGLE_UPDATE;
            break;
        default:
            dataUpdateFlag |= READ_UPDATE;
            break;
        }
        uiReg++;
    }
}
